#include <TimerThree.h>

#include <TimerOne.h>
#include <RGB_GLCD.h>
#include <avr/pgmspace.h>

#include <avr/interrupt.h>
#include <avr/io.h>
#define FASTADC 1
//#define CACHEOFF 1
#ifdef CACHEOFF
#define ISCACHED(myCheck)
#else
#define ISCACHED(myCheck) if(myCheck)
#endif
// defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
#if 0
#define DODELAY(myDelay) delay(myDelay);delayCounter-=myDelay
#else
#define DODELAY(myDelay)
#endif
#define BOTTOM 119
#define TOP 0
void DisplayLoop (void);
int int_counter = 0;
int oldSecond = 0;

long starttime = 0;
GLCD myGLCD;
int valueCache[4] = {
  0
};

unsigned char seg7cache[4][3][7];	//abc defg
unsigned char seg7cacheSign[4];
unsigned char segments[10] = {
  //  0    1    2    3    4    5    6    7   8    9
  0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x1f, 0x70, 0x7f, 0x7b
};

unsigned char seg7[7][4] = {
  {
   1, 0, 0, 0}
  ,				//seg a
  {
   1, 0, 1, 1}
  ,				//seg b
  {
   1, 1, 1, 2}
  ,				//seg c
  {
   1, 2, 0, 2}
  ,				//seg d
  {
   0, 2, 0, 1}
  ,				//seg e
  {
   0, 1, 0, 0}
  ,				//seg f
  {
   0, 1, 1, 1}
  //seg g
};

char printString[100];
char *printStringPos = printString;
void
qprint3 (int val)
{
  int d3 = val / 100;
  int d2 = val / 10;
  val -= d2 * 10;
  d2 -= d3 * 10;
  *printStringPos++ = '0' + d3;
  *printStringPos++ = '0' + d2;
  *printStringPos++ = '0' + val;
  *printStringPos++ = ' ';
}

void
printFlush (void)
{
  *printStringPos++ = 0;
  myGLCD.print (printString, LEFT, 119, 64, 64, 64);	//lower left
  printStringPos = printString;
  DODELAY (10);
}

//height is doubled 
void
sevenSegment (int value, int x, int y, int w, int h, int thickness, char pG,
	      char dG)
{
#ifndef CACHEOFF
  if (valueCache[pG] == value)
    return;
#endif
  valueCache[pG] = value;
  short segs = segments[value];
  short curbit = 0x40;
  char i = 0;
  unsigned char color = 0;	//black
  if (x < 0)
    {				//special case of 2&1/2 digit display
      if (value > 0)
	color = 255;		//white
      myGLCD.setColor (color, color, color);
      DODELAY (10);
      for (i = 1; i < 3; i++)
	{
	  ISCACHED (seg7cache[pG][dG][i] != color)
	  {
	    seg7cache[pG][dG][i] = color;
	    myGLCD.fillRect (x + w * seg7[i][0], y + h * seg7[i][1],
			     x + w * seg7[i][2] + thickness,
			     y + h * seg7[i][3] + thickness);
	  }
	}
      return;
    }
  if (value < 0)
    {
      myGLCD.setColor (color, color, color);
      DODELAY (10);
      for (; i < 7; i++)
	{
	  ISCACHED (seg7cache[pG][dG][i] != color)
	  {
	    seg7cache[pG][dG][i] = color;
	    myGLCD.fillRect (x + w * seg7[i][0], y + h * seg7[i][1],
			     x + w * seg7[i][2] + thickness,
			     y + h * seg7[i][3] + thickness);
	  }
	}
      return;
    }
  for (; i < 7; i++)
    {
      if (curbit & segs)
	color = 255;		//white
      else
	color = 0;
      DODELAY (10);
      myGLCD.setColor (color, color, color);
      ISCACHED (seg7cache[pG][dG][i] != color)
      {
	seg7cache[pG][dG][i] = color;
	myGLCD.fillRect (x + w * seg7[i][0], y + h * seg7[i][1],
			 x + w * seg7[i][2] + thickness,
			 y + h * seg7[i][3] + thickness);
      }
      curbit = curbit >> 1;
    }
}

int pGO[][5] = {
  {
   //    -35,13,38,24,5   
   -17, 13, 38, 24, 5}
  ,
  {
   //    6,72,28,18,3    
   19, 72, 28, 18, 3}
};

void
printSevens (int val, char pG)
{
  char sign = 1;
  if (val < 0)
    {
      val = -val;
      sign = -1;
    }
  if (val > 999)
    val = 999;
  int d3 = val / 100;
  int d2 = val / 10;
  val -= d2 * 10;
  d2 -= d3 * 10;
  int x = pGO[pG][0];
  int y = pGO[pG][1];
  int w = pGO[pG][2];
  int h = pGO[pG][3];
  int thickness = pGO[pG][4];
  if (seg7cacheSign[pG] != sign)
    {
      seg7cacheSign[pG] != sign;
      int color = 0;
      if (sign < 0)
	color = 255;		//white
      int i = 6;
      myGLCD.setColor (color, color, color);
      if (x < 0)
	myGLCD.fillRect (2, y + h * seg7[i][1],
			 x + w * seg7[i][2] - thickness,
			 y + h * seg7[i][3] + thickness);
      else
	myGLCD.fillRect (x - (w / 2) * seg7[i][2], y + h * seg7[i][1],
			 x - thickness, y + h * seg7[i][3] + thickness);
    }
  sevenSegment (d3, x, y, w, h, thickness, pG, 0);
  x += w + thickness * 4;
  sevenSegment (d2, x, y, w, h, thickness, pG, 1);
  x += w + thickness * 4;
  sevenSegment (val, x, y, w / 2, h / 2, thickness / 2, pG, 2);
}

#define IS_REVERSE(motorState) (motorState&0x1)
#define IS_PWM(motorState) (motorState<=ENABLE_REV)
#define IS_PWR(motorState) (motorState<=PWMOFF_REV)
#define IS_BRAKE_RELEASE(motorState) (motorState<=PWMOFF_REV)
#define IS_LOWDUTYCYCLE(motorState)  (motorState>REVERSE)
enum motorStateType
{
  FORWARD,			//pwm on,brake off
  REVERSE,			//pwm on,brake off
  ENABLE_FWD,			//pwm off , brake off
  ENABLE_REV,			//pwm off, brake off
  PWMOFF_FWD,			//pwm off , brake off
  PWMOFF_REV,			//pwm off, brake off
  PWROFF_FWD,			//pwm off , brake off
  PWROFF_REV,			//pwm off, brake off
  IDLE,				//disable PWM power on states below this//break on
  FAULTED,			//pwm off, brake on
  INVALID
};
char motorStateString[][12] = {
  "   Forward",			//pwm on,brake off
  "   Reverse",			//pwm on,brake off
  "Enable_Fwd",			//pwm off , brake off
  "Enable_Rev",			//pwm off, brake off
  "PWMoff_Fwd",			//pwm off , brake off
  "PWMoff_Rev",			//pwm off, brake off
  "PWRoff_Fwd",			//pwm off , brake off
  "PWRoff_Rev",			//pwm off, brake off
  "      Idle",			//disable PWM power on states below this//break on
  "   Faulted",			//pwm off, brake on
  "   Invalid"
};

//
//
//                                                        This block for varibles common between the loops
//
//
volatile motorStateType motorState = IDLE;	//idle=0
volatile int onTimer = 0;
volatile int aCurrent = 0;
volatile int bCurrent = 0;
volatile int uCommand = 0;
volatile int uRabbit = 0;
volatile int batVoltage = 0;
volatile int uCommandFiltered = 0;
volatile int lastuCurrentLimit = 0;
volatile int uCurrentLimit = 0;
volatile int uCurrentAverage = 0;
volatile int DebugValue = 0;
//display variables
//everything should be on-change to avoid unnecessary writes to screen
motorStateType lastMotorState = INVALID;	//idle=0
long lastRabbit = 0;
int lastBatVoltage = 1000;
long uCommandLong = 0;
#define BRAKE_RELEASE 29
#define PWMPOWER 37
#define PWMREVERSE 36
#define HEADLIGHTS 33
#define PWMENABLE 10
#define DEBUGLIGHT 9
#define READFAULT 35
#define READHEADLIGHTS 34
#define PWMA 2
#define PWMB 1
//
int
analogReadMulti (int port)
{
  analogRead (port);
  return analogRead (port);
}






